/*
 * KdTree.java
 * RVO2 Library Java
 *
 * Copyright 2008 University of North Carolina at Chapel Hill
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Please send all bug reports to <geom@cs.unc.edu>.
 *
 * The authors may be contacted via:
 *
 * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
 * Dept. of Computer Science
 * 201 S. Columbia St.
 * Frederick P. Brooks, Jr. Computer Science Bldg.
 * Chapel Hill, N.C. 27599-3175
 * United States of America
 *
 * <http://gamma.cs.unc.edu/RVO2/>
 */

package RVO;

import java.util.List;
import java.util.ArrayList;

/**
 * Defines k-D trees for agents and static obstacles in the simulation.
 */
class KdTree {
    /**
     * Defines a node of an agent k-D tree.
     */
    private static class AgentTreeNode {
        int begin_;
        int end_;
        int left_;
        int right_;
        float maxX_;
        float maxY_;
        float minX_;
        float minY_;
    }

    /**
     * Defines a pair of scalar values.
     */
    private static class FloatPair {
        private float a_;
        private float b_;

        /**
         * Constructs and initializes a pair of scalar values.
         *
         * @param a The first scalar value.
         * @param b The second scalar value.
         */
        FloatPair(float a, float b) {
            a_ = a;
            b_ = b;
        }

        /**
         * Returns true if the first pair of scalar values is less
         * than the second pair of scalar values.
         *
         * @param pair1 The first pair of scalar values.
         * @param pair2 The second pair of scalar values.
         * @return True if the first pair of scalar values is less than the
         * second pair of scalar values.
         */
        static boolean lessThan(FloatPair pair1, FloatPair pair2) {
            return pair1.a_ < pair2.a_ || !(pair2.a_ < pair1.a_) && pair1.b_ < pair2.b_;
        }

        /**
         * Returns true if the first pair of scalar values is less
         * than or equal to the second pair of scalar values.
         *
         * @param pair1 The first pair of scalar values.
         * @param pair2 The second pair of scalar values.
         * @return True if the first pair of scalar values is less than or
         * equal to the second pair of scalar values.
         */
        static boolean lessThanOrEqual(FloatPair pair1, FloatPair pair2) {
            return (pair1.a_ == pair2.a_ && pair1.b_ == pair2.b_) || lessThan(pair1, pair2);
        }

        /**
         * Returns true if the first pair of scalar values is
         * greater than the second pair of scalar values.
         *
         * @param pair1 The first pair of scalar values.
         * @param pair2 The second pair of scalar values.
         * @return True if the first pair of scalar values is greater than
         * the second pair of scalar values.
         */
        static boolean greaterThan(FloatPair pair1, FloatPair pair2) {
            return !lessThanOrEqual(pair1, pair2);
        }

        /**
         * Returns true if the first pair of scalar values is
         * greater than or equal to the second pair of scalar values.
         *
         * @param pair1 The first pair of scalar values.
         * @param pair2 The second pair of scalar values.
         * @return True if the first pair of scalar values is greater than
         * or equal to the second pair of scalar values.
         */
        static boolean greaterThanOrEqual(FloatPair pair1, FloatPair pair2) {
            return !lessThan(pair1, pair2);
        }
    }

    /**
     * Defines a node of an obstacle k-D tree.
     */
    private static class ObstacleTreeNode {
        Obstacle obstacle_;
        ObstacleTreeNode left_;
        ObstacleTreeNode right_;
    }

    /**
     * The maximum size of an agent k-D tree leaf.
     */
    private static final int MAX_LEAF_SIZE = 10;
    private Simulator simulator;
    private Agent[] agents_;
    private AgentTreeNode[] agentTree_;
    private ObstacleTreeNode obstacleTree_;

    public void setSimulator(Simulator simulator) {
        this.simulator = simulator;
    }

    /**
     * Builds an agent k-D tree.
     */
    void buildAgentTree() {
        if (agents_ == null || agents_.length != simulator.agents_.size()) {
            agents_ = new Agent[simulator.agents_.size()];

            for (int i = 0; i < agents_.length; ++i) {
                agents_[i] = simulator.agents_.get(i);
            }

            agentTree_ = new AgentTreeNode[2 * agents_.length];

            for (int i = 0; i < agentTree_.length; ++i) {
                agentTree_[i] = new AgentTreeNode();
            }
        }

        if (agents_.length != 0) {
            buildAgentTreeRecursive(0, agents_.length, 0);
        }
    }

    /**
     * Builds an obstacle k-D tree.
     */
    void buildObstacleTree() {
        obstacleTree_ = new ObstacleTreeNode();

        List<Obstacle> obstacles = new ArrayList<Obstacle>(simulator.obstacles_.size());

        for (int i = 0; i < simulator.obstacles_.size(); ++i) {
            obstacles.add(simulator.obstacles_.get(i));
        }

        obstacleTree_ = buildObstacleTreeRecursive(obstacles);
    }

    /**
     * Computes the agent neighbors of the specified agent.
     *
     * @param agent The agent for which agent neighbors are to be computed.
     * @param rangeSq The squared range around the agent.
     */
    void computeAgentNeighbors(Agent agent, float[] rangeSq) {
        queryAgentTreeRecursive(agent, rangeSq, 0);
    }

    /**
     * Computes the obstacle neighbors of the specified agent.
     *
     * @param agent The agent for which obstacle neighbors are to be computed.
     * @param rangeSq The squared range around the agent.
     */
    void computeObstacleNeighbors(Agent agent, float rangeSq) {
        queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_);
    }

    /**
     * Queries the visibility between two points within a specified radius.
     *
     * @param q1 The first point between which visibility is to be tested.
     * @param q2 The second point between which visibility is to be tested.
     * @param radius The radius within which visibility is to be tested.
     * @return True if q1 and q2 are mutually visible within the radius;
     * false otherwise.
     */
    boolean queryVisibility(Vector2 q1, Vector2 q2, float radius) {
        return queryVisibilityRecursive(q1, q2, radius, obstacleTree_);
    }

    int queryNearAgent(Vector2 point, float radius) {
        float[] rangeSq = {Float.MAX_VALUE};
        int[] agentNo = {-1};
        queryAgentTreeRecursive(point, rangeSq, agentNo, 0);
        if (rangeSq[0] < radius * radius)
            return agentNo[0];
        return -1;
    }

    /**
     * Recursive method for building an agent k-D tree.
     *
     * @param begin The beginning agent k-D tree node node index.
     * @param end The ending agent k-D tree node index.
     * @param node The current agent k-D tree node index.
     */
    private void buildAgentTreeRecursive(int begin, int end, int node) {
        agentTree_[node].begin_ = begin;
        agentTree_[node].end_ = end;
        agentTree_[node].minX_ = agentTree_[node].maxX_ = agents_[begin].position_.x_;
        agentTree_[node].minY_ = agentTree_[node].maxY_ = agents_[begin].position_.y_;

        for (int i = begin + 1; i < end; ++i) {
            agentTree_[node].maxX_ = Math.max(agentTree_[node].maxX_, agents_[i].position_.x_);
            agentTree_[node].minX_ = Math.min(agentTree_[node].minX_, agents_[i].position_.x_);
            agentTree_[node].maxY_ = Math.max(agentTree_[node].maxY_, agents_[i].position_.y_);
            agentTree_[node].minY_ = Math.min(agentTree_[node].minY_, agents_[i].position_.y_);
        }

        if (end - begin > MAX_LEAF_SIZE) {
            /* No leaf node. */
            boolean isVertical = agentTree_[node].maxX_ - agentTree_[node].minX_ > agentTree_[node].maxY_ - agentTree_[node].minY_;
            float splitValue = 0.5f * (isVertical ? agentTree_[node].maxX_ + agentTree_[node].minX_ : agentTree_[node].maxY_ + agentTree_[node].minY_);

            int left = begin;
            int right = end;

            while (left < right) {
                while (left < right && (isVertical ? agents_[left].position_.x_ : agents_[left].position_.y_) < splitValue) {
                    ++left;
                }

                while (right > left && (isVertical ? agents_[right - 1].position_.x_ : agents_[right - 1].position_.y_) >= splitValue) {
                    --right;
                }

                if (left < right) {
                    Agent tempAgent = agents_[left];
                    agents_[left] = agents_[right - 1];
                    agents_[right - 1] = tempAgent;
                    ++left;
                    --right;
                }
            }

            int leftSize = left - begin;

            if (leftSize == 0) {
                ++leftSize;
                ++left;
                ++right;
            }

            agentTree_[node].left_ = node + 1;
            agentTree_[node].right_ = node + 2 * leftSize;

            buildAgentTreeRecursive(begin, left, agentTree_[node].left_);
            buildAgentTreeRecursive(left, end, agentTree_[node].right_);
        }
    }

    /**
     * Recursive method for building an obstacle k-D tree.
     *
     * @param obstacles A list of obstacles.
     * @return An obstacle k-D tree node.
     */
    private ObstacleTreeNode buildObstacleTreeRecursive(List<Obstacle> obstacles) {
        if (obstacles.size() == 0) {
            return null;
        }

        ObstacleTreeNode node = new ObstacleTreeNode();

        int optimalSplit = 0;
        int minLeft = obstacles.size();
        int minRight = obstacles.size();

        for (int i = 0; i < obstacles.size(); ++i) {
            int leftSize = 0;
            int rightSize = 0;

            Obstacle obstacleI1 = obstacles.get(i);
            Obstacle obstacleI2 = obstacleI1.next_;

            /* Compute optimal split node. */
            for (int j = 0; j < obstacles.size(); ++j) {
                if (i == j) {
                    continue;
                }

                Obstacle obstacleJ1 = obstacles.get(j);
                Obstacle obstacleJ2 = obstacleJ1.next_;

                float j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
                float j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);

                if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON) {
                    ++leftSize;
                } else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON) {
                    ++rightSize;
                } else {
                    ++leftSize;
                    ++rightSize;
                }

                if (FloatPair.greaterThanOrEqual(new FloatPair(Math.max(leftSize, rightSize), Math.min(leftSize, rightSize)), new FloatPair(Math.max(minLeft, minRight), Math.min(minLeft, minRight)))) {
                    break;
                }
            }

            if (FloatPair.lessThan(new FloatPair(Math.max(leftSize, rightSize), Math.min(leftSize, rightSize)), new FloatPair(Math.max(minLeft, minRight), Math.min(minLeft, minRight)))) {
                minLeft = leftSize;
                minRight = rightSize;
                optimalSplit = i;
            }
        }

        {
            /* Build split node. */
            List<Obstacle> leftObstacles = new ArrayList<Obstacle>(minLeft);

            for (int n = 0; n < minLeft; ++n) {
                leftObstacles.add(null);
            }

            List<Obstacle> rightObstacles = new ArrayList<Obstacle>(minRight);

            for (int n = 0; n < minRight; ++n) {
                rightObstacles.add(null);
            }

            int leftCounter = 0;
            int rightCounter = 0;
            int i = optimalSplit;

            Obstacle obstacleI1 = obstacles.get(i);
            Obstacle obstacleI2 = obstacleI1.next_;

            for (int j = 0; j < obstacles.size(); ++j) {
                if (i == j) {
                    continue;
                }

                Obstacle obstacleJ1 = obstacles.get(j);
                Obstacle obstacleJ2 = obstacleJ1.next_;

                float j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
                float j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);

                if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON) {
                    leftObstacles.set(leftCounter++, obstacles.get(j));
                } else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON) {
                    rightObstacles.set(rightCounter++, obstacles.get(j));
                } else {
                    /* Split obstacle j. */
                    float t = RVOMath.det(Vector2.subtract(obstacleI2.point_, obstacleI1.point_), Vector2.subtract(obstacleJ1.point_, obstacleI1.point_)) / RVOMath.det(Vector2.subtract(obstacleI2.point_, obstacleI1.point_), Vector2.subtract(obstacleJ1.point_, obstacleJ2.point_));

                    Vector2 splitPoint = Vector2.add(obstacleJ1.point_, Vector2.multiply(Vector2.subtract(obstacleJ2.point_, obstacleJ1.point_), t));

                    Obstacle newObstacle = new Obstacle();
                    newObstacle.point_ = splitPoint;
                    newObstacle.previous_ = obstacleJ1;
                    newObstacle.next_ = obstacleJ2;
                    newObstacle.convex_ = true;
                    newObstacle.direction_ = obstacleJ1.direction_;

                    newObstacle.id_ = simulator.obstacles_.size();

                    simulator.obstacles_.add(newObstacle);

                    obstacleJ1.next_ = newObstacle;
                    obstacleJ2.previous_ = newObstacle;

                    if (j1LeftOfI > 0.0f) {
                        leftObstacles.set(leftCounter++, obstacleJ1);
                        rightObstacles.set(rightCounter++, newObstacle);
                    } else {
                        rightObstacles.set(rightCounter++, obstacleJ1);
                        leftObstacles.set(leftCounter++, newObstacle);
                    }
                }
            }

            node.obstacle_ = obstacleI1;
            node.left_ = buildObstacleTreeRecursive(leftObstacles);
            node.right_ = buildObstacleTreeRecursive(rightObstacles);

            return node;
        }
    }

    private void queryAgentTreeRecursive(Vector2 position, float[] rangeSq, int[] agentNo, int node) {
        if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE) {
            for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i) {
                float distSq = RVOMath.absSq(Vector2.subtract(position, agents_[i].position_));
                if (distSq < rangeSq[0]) {
                    rangeSq[0] = distSq;
                    agentNo[0] = agents_[i].id_;
                }
            }
        } else {
            float distSqLeft = RVOMath.sqr(Math.max(0.0f, agentTree_[agentTree_[node].left_].minX_ - position.x_)) + RVOMath.sqr(Math.max(0.0f, position.x_ - agentTree_[agentTree_[node].left_].maxX_)) + RVOMath.sqr(Math.max(0.0f, agentTree_[agentTree_[node].left_].minY_ - position.y_)) + RVOMath.sqr(Math.max(0.0f, position.y_ - agentTree_[agentTree_[node].left_].maxY_));
            float distSqRight = RVOMath.sqr(Math.max(0.0f, agentTree_[agentTree_[node].right_].minX_ - position.x_)) + RVOMath.sqr(Math.max(0.0f, position.x_ - agentTree_[agentTree_[node].right_].maxX_)) + RVOMath.sqr(Math.max(0.0f, agentTree_[agentTree_[node].right_].minY_ - position.y_)) + RVOMath.sqr(Math.max(0.0f, position.y_ - agentTree_[agentTree_[node].right_].maxY_));

            if (distSqLeft < distSqRight) {
                if (distSqLeft < rangeSq[0]) {
                    queryAgentTreeRecursive(position, rangeSq, agentNo, agentTree_[node].left_);

                    if (distSqRight < rangeSq[0]) {
                        queryAgentTreeRecursive(position, rangeSq, agentNo, agentTree_[node].right_);
                    }
                }
            } else {
                if (distSqRight < rangeSq[0]) {
                    queryAgentTreeRecursive(position, rangeSq, agentNo, agentTree_[node].right_);

                    if (distSqLeft < rangeSq[0]) {
                        queryAgentTreeRecursive(position, rangeSq, agentNo, agentTree_[node].left_);
                    }
                }
            }
        }
    }

    /**
     * Recursive method for computing the agent neighbors of the specified agent.
     *
     * @param agent The agent for which agent neighbors are to be computed.
     * @param rangeSq The squared range around the agent.
     * @param node The current agent k-D tree node index.
     */
    private void queryAgentTreeRecursive(Agent agent, float[] rangeSq, int node) {
        if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE) {
            for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i) {
                agent.insertAgentNeighbor(agents_[i], rangeSq);
            }
        } else {
            float distSqLeft = RVOMath.sqr(Math.max(0.0f, agentTree_[agentTree_[node].left_].minX_ - agent.position_.x_)) + RVOMath.sqr(Math.max(0.0f, agent.position_.x_ - agentTree_[agentTree_[node].left_].maxX_)) + RVOMath.sqr(Math.max(0.0f, agentTree_[agentTree_[node].left_].minY_ - agent.position_.y_)) + RVOMath.sqr(Math.max(0.0f, agent.position_.y_ - agentTree_[agentTree_[node].left_].maxY_));
            float distSqRight = RVOMath.sqr(Math.max(0.0f, agentTree_[agentTree_[node].right_].minX_ - agent.position_.x_)) + RVOMath.sqr(Math.max(0.0f, agent.position_.x_ - agentTree_[agentTree_[node].right_].maxX_)) + RVOMath.sqr(Math.max(0.0f, agentTree_[agentTree_[node].right_].minY_ - agent.position_.y_)) + RVOMath.sqr(Math.max(0.0f, agent.position_.y_ - agentTree_[agentTree_[node].right_].maxY_));

            if (distSqLeft < distSqRight) {
                if (distSqLeft < rangeSq[0]) {
                    queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left_);

                    if (distSqRight < rangeSq[0]) {
                        queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right_);
                    }
                }
            } else {
                if (distSqRight < rangeSq[0]) {
                    queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right_);

                    if (distSqLeft < rangeSq[0]) {
                        queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left_);
                    }
                }
            }
        }
    }

    /**
     * Recursive method for computing the obstacle neighbors of the specified agent.
     *
     * @param agent The agent for which obstacle neighbors are to be computed.
     * @param rangeSq The squared range around the agent.
     * @param node The current obstacle k-D node.
     */
    private void queryObstacleTreeRecursive(Agent agent, float rangeSq, ObstacleTreeNode node) {
        if (node != null) {
            Obstacle obstacle1 = node.obstacle_;
            Obstacle obstacle2 = obstacle1.next_;

            float agentLeftOfLine = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, agent.position_);

            queryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0.0f ? node.left_ : node.right_);

            float distSqLine = RVOMath.sqr(agentLeftOfLine) / RVOMath.absSq(Vector2.subtract(obstacle2.point_, obstacle1.point_));

            if (distSqLine < rangeSq) {
                if (agentLeftOfLine < 0.0f) {
                    /*
                     * Try obstacle at this node only if agent is on right side of
                     * obstacle (and can see obstacle).
                     */
                    agent.insertObstacleNeighbor(node.obstacle_, rangeSq);
                }

                /* Try other side of line. */
                queryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0.0f ? node.right_ : node.left_);
            }
        }
    }

    /**
     * Recursive method for querying the visibility between two points within a specified radius.
     *
     * @param q1 The first point between which visibility is to be tested.
     * @param q2 The second point between which visibility is to be tested.
     * @param radius The radius within which visibility is to be tested.
     * @param node The current obstacle k-D node.
     * @return True if q1 and q2 are mutually visible within the radius; false otherwise.
     */
    private boolean queryVisibilityRecursive(Vector2 q1, Vector2 q2, float radius, ObstacleTreeNode node) {
        if (node == null) {
            return true;
        }

        Obstacle obstacle1 = node.obstacle_;
        Obstacle obstacle2 = obstacle1.next_;

        float q1LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q1);
        float q2LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q2);
        float invLengthI = 1.0f / RVOMath.absSq(Vector2.subtract(obstacle2.point_, obstacle1.point_));

        if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) {
            return queryVisibilityRecursive(q1, q2, radius, node.left_) && ((RVOMath.sqr(q1LeftOfI) * invLengthI >= RVOMath.sqr(radius) && RVOMath.sqr(q2LeftOfI) * invLengthI >= RVOMath.sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node.right_));
        }

        if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) {
            return queryVisibilityRecursive(q1, q2, radius, node.right_) && ((RVOMath.sqr(q1LeftOfI) * invLengthI >= RVOMath.sqr(radius) && RVOMath.sqr(q2LeftOfI) * invLengthI >= RVOMath.sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node.left_));
        }

        if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) {
            /* One can see through obstacle from left to right. */
            return queryVisibilityRecursive(q1, q2, radius, node.left_) && queryVisibilityRecursive(q1, q2, radius, node.right_);
        }

        float point1LeftOfQ = RVOMath.leftOf(q1, q2, obstacle1.point_);
        float point2LeftOfQ = RVOMath.leftOf(q1, q2, obstacle2.point_);
        float invLengthQ = 1.0f / RVOMath.absSq(Vector2.subtract(q2, q1));

        return point1LeftOfQ * point2LeftOfQ >= 0.0f && RVOMath.sqr(point1LeftOfQ) * invLengthQ > RVOMath.sqr(radius) && RVOMath.sqr(point2LeftOfQ) * invLengthQ > RVOMath.sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node.left_) && queryVisibilityRecursive(q1, q2, radius, node.right_);
    }
}
